 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
VEXPro range motor control loop
Description: This tutorial describes how to use interrupts to accurately measure range using an ultrasonic rangefinder, then goes on to apply that in a motor control system with the loop controller running in a ROS node.Tutorial Level: INTERMEDIATE
Contents
This tutorial takes you step by step from measuring range and controlling a motor through an integrated closed-loop control system for maintaining an object at a constant distance. No steering logic is introduced, so it is only useful in one dimension.
Four programs are presented in this tutorial, and the code for three are explained:
- VEXProRangePublish: a program which uses interrupts to accurately measure the range of an object using an ultrasonic rangefinder, and publishes the range. This program is explained in depth.
- VEXProMotor13Subscribe: a program which subscribes to a motor speed topic and drives the VEXPro 2-wire motor output to the requested speed.
- VEXProRangeMotorLoop: a program which combines the first two into a system that publishes the range and subscribes to motor speed requests published by the fourth program. The differences between this and VEXProRangePublish are explained in depth, showing how motor control is integrated with rangefinding.
- VEXProRangeMotorCtrlr: a ROS node written in C++ which attempts to keep an object at a constant distance by running a motor forward or backward as the object becomes nearer or further away than the setpoint. With appropriate mechanics it can be the controller part of a closed-loop control system. This program is explained in depth.
The code
Listings for three programs are presented here, along with explanations of their operation. If you understand the first: VEXProRangePublish, you'll easily see how motor control is integrated into the second: VEXProRangeMotorLoop. The third is the C++ code for the ROS node which does the closed-loop control. See "The code explained" section for line-by-line explanations of each of these programs. The testing section describes how to run all four programs and test them individually.
The code for all four programs is in the examples directory and you should be able to open them in your Terk IDE.
VEXProRangePublish
This program simply publishes the range to an obstacle sensed by an ultrasonic rangefinder.
1 /*
2 * VEXProRangePublish.cpp
3 *
4 * Created on: Jul 12, 2012
5 * Author: bouchier
6 *
7 * Publish the range from an ultrasonic ranging sensor connected to digital 1 (Input) &
8 * digital 2 (Output) on topic sonar1
9 */
10
11 #include <ros.h>
12 #include <std_msgs/Int32.h>
13 #include <stdio.h>
14 #include <unistd.h>
15 #include "qegpioint.h"
16
17 ros::NodeHandle nh;
18 std_msgs::Int32 range;
19 ros::Publisher sonar1("sonar1", &range);
20
21 char *rosSrvrIp = "192.168.11.9";
22
23 #define USPI 150
24 #define BIAS 300
25
26 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
27 {
28 long val;
29
30 val = ptv1->tv_usec - ptv0->tv_usec;
31 val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
32
33 return val;
34 }
35
36 void callback(unsigned int io, struct timeval *ptv, void *userdata)
37 {
38 static struct timeval tv0;
39 static int flag = 0;
40 int sonarVal;
41
42 if (io==0)
43 {
44 flag = 1;
45 tv0 = *ptv;
46 }
47
48 if (io==1 && flag)
49 {
50 sonarVal = diff(&tv0, ptv);
51 if (sonarVal>BIAS)
52 sonarVal = (sonarVal-BIAS)/USPI;
53 range.data = sonarVal;
54 printf("%d\n", sonarVal);
55 }
56 }
57
58 // Note: connector labeled "INPUT" on sonar sensor goes to
59 // digital 1 (bit 0), and connector labeled "OUTPUT" goes to
60 // digital 2 (bit 1).
61 int main()
62 {
63 CQEGpioInt &gpio = CQEGpioInt::GetRef();
64 volatile unsigned int d;
65
66 // reset bit 0, set as output for sonar trigger
67 gpio.SetData(0x0000);
68 gpio.SetDataDirection(0x0001);
69
70 // set callbacks on negative edge for both bits 0 (trigger)
71 // and 1 (echo)
72 gpio.RegisterCallback(0, NULL, callback);
73 gpio.RegisterCallback(1, NULL, callback);
74 gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
75 gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
76
77 //nh.initNode();
78 nh.initNode(rosSrvrIp);
79 nh.advertise(sonar1);
80
81 // trigger sonar by toggling bit 0
82 while(1)
83 {
84 gpio.SetData(0x0001);
85 for (d=0; d<120000; d++);
86 gpio.SetData(0x0000);
87 sleep(1); // the interrupt breaks us out of this sleep
88 sleep(1); // now really sleep a second
89 sonar1.publish( &range );
90 nh.spinOnce();
91 }
92 }
The code explained
Now let's break the code down. Here's how VEXProRangePublish works.
In addition to the std_msgs/Int32.h message used to publish the range, you include the libqwerk definition of the GPIO with Interrupts library: qegpioint.h. This gives control over the digital I/O pins on the VEXPro, and enables assigning callbacks to their interrupt.
The declaration of the publisher on the sonar1 topic at line 19 is similar to the Hello ROS example publisher.
The diff() function takes two linux timeval structs and subtracts them. Timeval presents time in two values representing seconds and microseconds. The time difference is return in microseconds. This function is used to calculate the time difference between the emitted and returning ultrasonic pulse.
36 void callback(unsigned int io, struct timeval *ptv, void *userdata)
37 {
38 static struct timeval tv0;
39 static int flag = 0;
40 int sonarVal;
41
42 if (io==0)
43 {
44 flag = 1;
45 tv0 = *ptv;
46 }
47
48 if (io==1 && flag)
49 {
50 sonarVal = diff(&tv0, ptv);
51 if (sonarVal>BIAS)
52 sonarVal = (sonarVal-BIAS)/USPI;
53 range.data = sonarVal;
54 printf("%d\n", sonarVal);
55 }
56 }
The callback() function is called at interrupt level on the negative edge of the output and input pins of the rangefinder. This happens first when the code in main() drives the output negative (the start of the acoustic pulse), and a second time when the echo is received. On the first call, it stores the current time in tv0. On the second call, it subtracts tv0 from the current time, divides it by twice the speed of sound in microseconds per inch (USPI), with the result being inches to the reflective surface. It prints the value and stores it in the data part of the range message, which will later be published by main();
63 CQEGpioInt &gpio = CQEGpioInt::GetRef();
Here you declare the GPIO object so you can use its methods to operate on the digital pins of the VEXPro controller.
66 // reset bit 0, set as output for sonar trigger
67 gpio.SetData(0x0000);
68 gpio.SetDataDirection(0x0001);
69
70 // set callbacks on negative edge for both bits 0 (trigger)
71 // and 1 (echo)
72 gpio.RegisterCallback(0, NULL, callback);
73 gpio.RegisterCallback(1, NULL, callback);
74 gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
75 gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
main() sets up the digital I/O pins so that digital port 1 (which corresponds to bit 0) is an output, and both are low. It then registers callback() to be called when either digital port 1 or 2 (bits 0 or 1) transition low. These trigger the timing measurement in callback() described above.
The node initialization and publisher advertisement on lines 78 - 79 work as described in the Hello ROS tutorial.
The program then loops forever in the while(1) loop, only breaking out when you terminate execution, e.g. with ^C or by stopping it with the IDE. First it sets digital pin 1 high, spins a while while the rangefinder sets up, then sets the pin low, which triggers the rangefinder to emit the ultrasonic pulse, and also triggers the first time measurement by callback(). It then sleeps. The interrupt that happens when the reflected ultrasonic pulse returns and digital pin 2 (the input) goes low triggers the second time measurement and range calculation by callback(), but also breaks main() out of its sleep. The second sleep() statement on line 88 ensures the program actually sleeps for a second between range measurements. When the sleep ends, the program publishes the range, and calls spinOnce() which sends the message to ROS (and checks for any other work that needs doing).
VEXProRangeMotorLoop
This program integrates motor operation into the rangefinder program above.
1 /*
2 * VEXProRangeMotorLoop.cpp
3 *
4 * Created on: Jul 13, 2012
5 * Author: bouchier
6 * Publish the range from a Sonar connected to digital 1 (Input) &
7 * digital 2 (Output) on topic sonar1. Control motor 13 speed by
8 * publishing the desired speed on a ros topic with e.g.
9 * $ rostopic pub my_topic std_msgs/Int32 120.
10 * Drives the motor on VEXPro motor13 connection to the requested value: -255 to +255
11 * that is received on subscribed topic motor1
12 *
13 * Note: connector labeled "INPUT" on sonar sensor goes to
14 * digital 1 (bit 0), and connector labeled "OUTPUT" goes to
15 * digital 2 (bit 1).
16 */
17
18
19 #include <ros.h>
20 #include <std_msgs/Int32.h>
21 #include <stdio.h>
22 #include <unistd.h>
23 #include <qegpioint.h>
24 #include <qemotoruser.h>
25
26 ros::NodeHandle nh;
27 std_msgs::Int32 range;
28 ros::Publisher sonar1("sonar1", &range);
29 CQEMotorUser &motor = CQEMotorUser::GetRef(); // motor singleton
30 CQEGpioInt &gpio = CQEGpioInt::GetRef(); // GPIO singleton
31
32 char *rosSrvrIp = "192.168.11.9";
33
34 #define USPI 150
35 #define BIAS 300
36
37 /*
38 * Motor callback - called when new motor speed is published
39 */
40 void motorCb(const std_msgs::Int32& motor13_msg){
41 int speed = motor13_msg.data;
42 printf("Received subscribed motor speed %d\n", speed);
43 motor.SetPWM(0, speed);
44 }
45 ros::Subscriber<std_msgs::Int32> motorSub("motor13", motorCb );
46
47 /*
48 * Calculate difference in usec between sonar start & end timevals
49 */
50 unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
51 {
52 long val;
53
54 val = ptv1->tv_usec - ptv0->tv_usec;
55 val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;
56
57 return val;
58 }
59
60 /*
61 * Sonar callback. Called at interrupt level when sonar output transitions,
62 * indicating end of range measurement
63 */
64 void callback(unsigned int io, struct timeval *ptv, void *userdata)
65 {
66 static struct timeval tv0;
67 static int flag = 0;
68 int sonarVal;
69
70 if (io==0)
71 {
72 flag = 1;
73 tv0 = *ptv;
74 }
75
76 if (io==1 && flag)
77 {
78 sonarVal = diff(&tv0, ptv);
79 if (sonarVal>BIAS)
80 sonarVal = (sonarVal-BIAS)/USPI;
81 range.data = sonarVal;
82 //printf("%d\n", sonarVal);
83 }
84 }
85
86 int main()
87 {
88 volatile unsigned int d;
89
90 /* initialize ROS & subscribers & publishers */
91 //nh.initNode();
92 nh.initNode(rosSrvrIp);
93 nh.advertise(sonar1); // advertise sonar range topic
94 nh.subscribe(motorSub); // subscribe to motor speed topic
95
96 // reset bit 0, set as output for sonar trigger
97 gpio.SetData(0x0000);
98 gpio.SetDataDirection(0x0001);
99
100 // set callbacks on negative edge for both bits 0 (trigger)
101 // and 1 (echo)
102 gpio.RegisterCallback(0, NULL, callback);
103 gpio.RegisterCallback(1, NULL, callback);
104 gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
105 gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);
106
107 // trigger sonar by toggling bit 0
108 while(1)
109 {
110 gpio.SetData(0x0001);
111 for (d=0; d<120000; d++);
112 gpio.SetData(0x0000);
113 usleep(100000); // the interrupt breaks us out of this sleep
114 usleep(100000); // now really sleep
115 sonar1.publish( &range );
116 nh.spinOnce();
117 }
118 }
The code explained
The VEXProRangeMotorLoop program is only a small extension of the VEXProRangePublish program, having the addition of a motor speed subscriber. This section describes just the motor speed logic extensions; refer to the VEXProRangePublish description for the rest of the code. In fact, the motor speed subscriber uses the same logic previously described in the Example Subscriber tutorial, so we will only talk about how to control a motor using libqwerk.
In addition to the std_msgs/Int32.h message used to publish the requested motor speed, you include the libqwerk motor control library declarations, which provide access to the speed control methods.
You get a reference to the motor control object, whose methods you will use.
37 /*
38 * Motor callback - called when new motor speed is published
39 */
40 void motorCb(const std_msgs::Int32& motor13_msg){
41 int speed = motor13_msg.data;
42 printf("Received subscribed motor speed %d\n", speed);
43 motor.SetPWM(0, speed);
44 }
45 ros::Subscriber<std_msgs::Int32> motorSub("motor13", motorCb );
The motor speed callback is called whenever a message on the subscribed topic "motor13" is received. It takes the requested speed out of the Int32 message data and calls the motor's SetPWM() method. SetPWM() controls the pulse-width modulation of motor 13's H-bridge. The first argument is the motor index: 0 is motor 13, the first 2-wire motor port on the VEXPro. The valid range of speed values is -255 to +255, corresponding to full backward to full forward; 0 is stopped. See the libqwerk motor control library for a full list of available methods and arguments.
After defining the callback, the code instantiates the subscriber, telling it the topic name and callback function.
One other difference from VEXProRangePublish: this program sleeps in main() for only 100ms, giving it a much faster publish rate, and making the control loop faster.
That's it! Pretty straightforward. Next we'll look into the code that runs under ROS, and that subscribes to the range messages and publishes the motor speed messages to VEXProRangeMotorLoop.
VEXProRangeMotorCtrlr
This program runs as a ROS node on your ROS workstation and subscribes to rangefinder readings. Depending on whether the range is closer or further away than 20 inches, it runs the motor one way or the other - faster the further from the setpoint of 20 inches.
1 #include "ros/ros.h"
2 #include "std_msgs/Int32.h"
3
4 int range = 20;
5
6 void sonarCallback(const std_msgs::Int32::ConstPtr& sonarMsg)
7 {
8 ROS_INFO("sonar range: [%d]", sonarMsg->data);
9 range = sonarMsg->data;
10 }
11
12 int
13 main (int argc, char **argv)
14 {
15 // inialize ros
16 ros::init (argc, argv, "VEXProRangeMotorCtrlr");
17 ros::NodeHandle n;
18 ros::Rate loop_rate (2);
19 std_msgs::Int32 speedMsg;
20 std_msgs::Int32 positionMsg;
21
22 // declare the publishers & subscribers
23 ros::Publisher motor13_pub =
24 n.advertise < std_msgs::Int32 > ("motor13", 1000);
25 ros::Subscriber sonarSub = n.subscribe("sonar1", 1000, sonarCallback);
26
27 int speed = 0;
28
29 while (ros::ok ())
30 {
31 speed = std::min(range, 40); // clamp range to a max
32 speed = (speed * 10) - 200; // give it a range of -200 - +200, with 0 at 20"
33 speedMsg.data = speed;
34
35 ROS_INFO ("range %d speed %d", range, speed);
36 motor13_pub.publish (speedMsg);
37
38 ros::spinOnce ();
39
40 loop_rate.sleep ();
41 }
42 return 0;
43 }
The code explained
The VEXProRangeMotorCtrlr program runs on your ROS workstation. The executable is built when you rosmake the rosserial_embeddedlinux package on your ROS workstation, and is written into the bin directory.
As usual, you include the message types in use; in this case Int32 is used for both range and motor speed.
4 int range = 20;
range is the current object distance; it is written by the subscriber callback function and read by the main() loop and motor control is based on this value.
Just as on the VEXPro side, the ROS side uses callbacks to notify a subscriber that a message was received. sonarCallback() copies the range reading that was published by the VEXProRangeMotorLoop program on VEXPro into the range variable.
You can find an in-depth explanation of how this ROS subscriber code works in this tutorial.
The node initializes its ROS subsystem. It gets a ROS Nodehandle, which gives access to methods to subscribe, publish, etc. It sets its loop-rate to 2Hz, and it declares the messages which will be used to receive range and send motor speed requests.
Next, the program instantiates the motor publisher and range subscriber, passing them the topic name and callback for the subscriber. Instantiating the publisher and subscriber causes them to ask the ROS master for the location of the node publishing or subscribing on those topics - this is the rosserial_python.py node instance acting as a proxy for VEXProRangeMotorLoop.
Note that the system supports multiple rosserial_python proxy nodes servicing (usually) different topics.
29 while (ros::ok ())
30 {
31 speed = std::min(range, 40); // clamp range to a max
32 speed = (speed * 10) - 200; // give it a range of -200 - +200, with 0 at 20"
33 speedMsg.data = speed;
34
35 ROS_INFO ("range %d speed %d", range, speed);
36 motor13_pub.publish (speedMsg);
37
38 ros::spinOnce ();
39
40 loop_rate.sleep ();
41 }
The program enters an endless loop, in which range is converted to speed in lines 31-32 by limiting it to 0 to 40 inches, then the distance range expanded to a speed range of -200 to +200. 20 inches produces a speed of 0.
The program publishes the motor speed, calls spinOnce() to let messages get sent, then sleeps for half a second in loop_rate.sleep() as long as ROS is running and it hasn't been terminated.
Testing the code
This section has three testing sections that use the programs described in this tutorial.
Testing the rangefinder
Plug the input connector of the VEX ultrasonic rangefinder into digital port 1, and the output connector into digital port 2. Like most ultrasonic rangefinders, the Vex unit uses a trigger pulse to start the ranging pulse, and emits a pulse when it detects the returning acoustic reflection. The time between the falling edge of the trigger and the falling edge of the output is the round-trip time of the acoustic wave, from which the range to the reflective surface can be calculated.
As usual, make sure ros_core and serial_node.py are running. Build, download, and start the VEXProRangePublish binary to the VEXPro controller.
Use rostopic to echo the range measurement (in inches) that's published by the VEXProRangePublish program once a second. Change the distance to the reflective surface and watch the range change.
$ rostopic echo sonar1 data: 38 --- data: 38 --- data: 39 --- data: 15 --- data: 15 --- data: 14 ---
Testing the motor
Plug a 2-wire motor into the Motor13 port on the VEXPro. Note: Vex motors have plugs with male pins. You can adapt a 3rd-party motor by using a servo extender cable and some solder - just remove the sheath around the pins on the servo extender cable. The 2-wire motor connects to the two pins furthest from the edge of the controller (corresponding to the control and +5V pins of a servo or 3-wire motor). The controller pulse-width-modulates battery voltage to the motor pins through an H-bridge protected by a self-resetting fuse.
Build, download, and start the VEXProMotor13Subscribe binary to the VEXPro controller. Use rostopic to publish the desired motor speed to the listening subscriber. Note the use of -- preceding a negative speed. The valid speed range is -255 to +255.
$ rostopic pub -1 motor13 std_msgs/Int32 100 publishing and latching message for 3.0 seconds $ rostopic pub -1 motor13 std_msgs/Int32 -- -100 publishing and latching message for 3.0 seconds $ rostopic pub -1 motor13 std_msgs/Int32 0 publishing and latching message for 3.0 seconds
The program running on the VEXPro prints each speed as it receives it:
root@qwerk:~# /opt/usr/bin/VEXProMotor13Subscribe Connecting to TCP server at 192.168.11.9:11411.... connected to server Received subscribed motor speed 100 Received subscribed motor speed -100 Received subscribed motor speed 0
Pulling it all together: responsive control
This test builds on the two previous tests by running
- VEXProRangeMotorLoop on the VEXPro, which publishes sonar readings and subscribes to the motor speed topic
- VEXProRangeMotorCtrlr on the ROS workstation, which reads the sonar topic and responds by publishing motor speed/direction requests.
Build, download, and start the VEXProRangeMotorLoop binary to the VEXPro controller. It will sit publishing range and waiting for the first motor speed message.
root@qwerk:~# /opt/usr/bin/VEXProMotorRangeLoop Connecting to TCP server at 192.168.11.9:11411.... connected to server
The VEXProMotorRangeCtrlr program was built when you build the embedded linux package, and the binary is in the rosserial_embeddedlinux/bin directory. Run it on your ROS workstation. The motor connected to the VEXPro will start running. Move a reflective surface nearer or further from the rangefinder and watch the motor speed and direction change.
$ rosrun rosseal_embeddedlinux VEXProRangeMotorCtrlr [ INFO] [1348455399.015726663]: range 20 speed 0 [ INFO] [1348455399.501250299]: range 20 speed 0 [ INFO] [1348455399.501761423]: sonar range: [28] [ INFO] [1348455399.502253554]: sonar range: [28] [ INFO] [1348455400.001241799]: range 28 speed 80 [ INFO] [1348455400.001640643]: sonar range: [28] [ INFO] [1348455400.001790349]: sonar range: [28] [ INFO] [1348455400.002211817]: sonar range: [28] [ INFO] [1348455400.002438890]: sonar range: [28] [ INFO] [1348455402.001361698]: range 27 speed 70 [ INFO] [1348455402.002070288]: sonar range: [26] [ INFO] [1348455402.002388693]: sonar range: [25] [ INFO] [1348455402.002700115]: sonar range: [23] [ INFO] [1348455402.002869652]: sonar range: [22] [ INFO] [1348455402.501255444]: range 22 speed 20 [ INFO] [1348455402.501669090]: sonar range: [21] [ INFO] [1348455402.501814328]: sonar range: [21] [ INFO] [1348455402.502216803]: sonar range: [20] [ INFO] [1348455402.502413712]: sonar range: [19] [ INFO] [1348455403.001236051]: range 19 speed -10 [ INFO] [1348455403.001656959]: sonar range: [19] [ INFO] [1348455403.001812251]: sonar range: [18] [ INFO] [1348455403.003050679]: sonar range: [18] [ INFO] [1348455403.003198150]: sonar range: [17] [ INFO] [1348455403.501292350]: range 17 speed -30 [ INFO] [1348455403.501671922]: sonar range: [17] [ INFO] [1348455403.502039764]: sonar range: [17] [ INFO] [1348455403.502313201]: sonar range: [16] [ INFO] [1348455403.502652833]: sonar range: [16] [ INFO] [1348455404.001357309]: range 16 speed -40 [ INFO] [1348455404.001723474]: sonar range: [16] [ INFO] [1348455404.002102488]: sonar range: [15] [ INFO] [1348455404.002332354]: sonar range: [15] [ INFO] [1348455404.002524794]: sonar range: [15] [ INFO] [1348455404.501225078]: range 15 speed -50
VEXProRangeMotorLoop running on the VEXPro prints the requested motor speed:
Received subscribed motor speed 0 Received subscribed motor speed 80 Received subscribed motor speed 70 Received subscribed motor speed 20 Received subscribed motor speed -10 Received subscribed motor speed -30 Received subscribed motor speed -40 Received subscribed motor speed -50
